package clubhouse.motioncommand;


import java.awt.geom.Point2D;
import java.util.Vector;

import clubhouse.base.Base;
import clubhouse.base.Landmarks;

public class MotionPlan {
	
	private Integer pulse_dist;
	private Integer pulse_rot;
	private Integer trigo;
	private Integer index_current_dest;
	private Integer scenario;
	private double[] destinationCoord;
	
	private DecisionMaker decider;
	private CalculInfo calculator;
	
	public Vector<Point2D[]> getPolygons(){
		return(this.decider.polygons);
	}
	
	public Integer getPulse_dist() {
		return this.pulse_dist;
	}

	public Integer getPulse_rot() {
		return this.pulse_rot;
	}

	public Integer getTrigo() {
		return this.trigo;
	}
	
	public double[] getDestinationCoord() {
		return this.destinationCoord;
	}

	public double getDistance() {
		return (this.pulse_dist/calculator.delta);
	}

	public double getRotation() {
		return (this.pulse_rot/calculator.omega);
	}
	
	public MotionPlan(Integer scenario) {
		super();
		
		this.pulse_dist = 0;
		this.pulse_rot = 0;
		this.trigo = 0;
		this.index_current_dest = 1;
		this.decider = new DecisionMaker(scenario);
		this.calculator = new CalculInfo();
		this.destinationCoord = new double[2];
			
		this.scenario = scenario;	//=1 or 2

	}
	
	public void getNextPulses(double x_robot, double y_robot, double r_robot, Landmarks landmarks) throws Exception{
		if (Base.DEBUG) System.out.println("(getNextPulses) Scenario "+this.scenario);
		if (this.scenario == 1){		//Scenario 1 : "Follow a trajectory"
			//Get index of next destination landmark
			this.index_current_dest = this.decider.getNextIndex(x_robot,y_robot,landmarks.getLandmark(this.index_current_dest),this.index_current_dest);
			this.destinationCoord[0] = landmarks.getLandmark(this.index_current_dest)[1];
			this.destinationCoord[1] = landmarks.getLandmark(this.index_current_dest)[2];
			
			//Calculate the information to reach this destination
			this.calculator.calculate_move_info(x_robot,y_robot,r_robot,this.destinationCoord);
			
		} else {						//Scenario 2 : "Avoid the sensors"

			this.destinationCoord = this.decider.getNextPoint(landmarks, x_robot, y_robot,r_robot);
			//Calculate the information to reach this destination
	        this.calculator.calculate_move_info(x_robot,y_robot,r_robot,this.destinationCoord);
	        
		}
		
		//Put results in variables
		this.pulse_dist = this.calculator.getPulse_dist();
		this.pulse_rot = this.calculator.getPulse_rot();
		this.trigo = this.calculator.getTrigo();
	}

}
